#include "Defines.inc"
Global Integer PalletCount
Function GetCurrentPointID As Integer
	Integer i, j
	Real f1
	Real f2
	f2 = Dist(P(0), CurPos)
	For i = 1 To MAX_ID
		f1 = Dist(P(i), CurPos)
		If f1 < f2 Then
			j = i
			f2 = f1
		EndIf
	Next
	For i = 1 To MAX_TRAY
	  f1 = Dist(Pallet(1, i), CurPos)
		If f1 < f2 Then
			j = PREOK_ID
			f2 = f1
		EndIf
	  f1 = Dist(Pallet(2, i), CurPos)
		If f1 < f2 Then
			j = PREOK_ID
			f2 = f1
		EndIf
	Next
'	Go P(j)
	GetCurrentPointID = j
Fend
Function RobotInit
	Cls
	Reset
	Motor On
	Power High
	Speed 100
	Accel 100, 100
	Pallet 1, PTPALLET1, PTPALLET2, PTPALLET3, TRAY_COLUMNS, TRAY_ROWS
	Pallet 2, PTPALLET4, PTPALLET5, PTPALLET6, TRAY_COLUMNS, TRAY_ROWS
Fend

Function GetPalletNum As Integer
	If PalletCount >= 54 Then
		PalletCount = 1
	Else
		PalletCount = PalletCount + 1
	EndIf
   	GetPalletNum = PalletCount
Fend

Function SetSpeed(index As Integer)
	Integer a
	a = In(index)
	If a > 0 And a <= 100 Then
		Speed a
	EndIf
Fend




